from Methods.block import *

def dian2(p1,p2,width,height):#第二种即两个中心点z不一样
    u=0
    p3=np.zeros(3)
    p4=np.zeros(3)
    p5=np.zeros(3)
    p6=np.zeros(3)
    p_1=np.zeros(3)
    p_2=np.zeros(3)
    p_3=np.zeros(3)
    p_4=np.zeros(3)
    p_5=np.zeros(3)
    p_6=np.zeros(3)
    p_7=np.zeros(3)
    p_8=np.zeros(3)
    if p1[1]-p2[1]==0:
        u=1
    if u==0:
        k=-(p1[0]-p2[0])/(p1[1]-p2[1])
        x=width/(2*(1+k**2)**0.5)
        p3[0]=p1[0]+x
        p3[1]=p1[1]+k*x
        p3[2]=p1[2]
        p4[0]=p1[0]-x
        p4[1]=p1[1]-k*x
        p4[2]=p1[2]
        p5[0]=p2[0]+x
        p5[1]=p2[1]+k*x
        p5[2]=p2[2]
        p6[0]=p2[0]-x
        p6[1]=p2[1]-k*x
        p6[2]=p2[2]
    else :
        p3[0]=p1[0]
        p3[1]=p1[1]+width/2
        p3[2]=p1[2]
        p4[0]=p1[0]
        p4[1]=p1[1]-width/2
        p4[2]=p1[2]
        p5[0]=p2[0]
        p5[1]=p2[1]+width/2
        p5[2]=p2[2]
        p6[0]=p2[0]
        p6[1]=p2[1]-width/2
        p6[2]=p2[2]
    a=(p2[1]-p1[1])*(p3[2]-p1[2])-(p3[1]-p1[1])*(p2[2]-p1[2])
    b=(p2[2]-p1[2])*(p3[0]-p1[0])-(p3[2]-p1[2])*(p2[0]-p1[0])
    c=(p2[0]-p1[0])*(p3[1]-p1[1])-(p3[0]-p1[0])*(p2[1]-p1[1])
    t=height/(2*(a**2+b**2+c**2)**0.5)
    p_3[0]=p3[0]+a*t
    p_3[1]=p3[1]+b*t
    p_3[2]=p3[2]+c*t
    p_2[0]=p3[0]-a*t
    p_2[1]=p3[1]-b*t
    p_2[2]=p3[2]-c*t
    p_4[0]=p4[0]+a*t
    p_4[1]=p4[1]+b*t
    p_4[2]=p4[2]+c*t
    p_1[0]=p4[0]-a*t
    p_1[1]=p4[1]-b*t
    p_1[2]=p4[2]-c*t
    p_7[0]=p5[0]+a*t
    p_7[1]=p5[1]+b*t
    p_7[2]=p5[2]+c*t
    p_6[0]=p5[0]-a*t
    p_6[1]=p5[1]-b*t
    p_6[2]=p5[2]-c*t
    p_8[0]=p6[0]+a*t
    p_8[1]=p6[1]+b*t
    p_8[2]=p6[2]+c*t
    p_5[0]=p6[0]-a*t
    p_5[1]=p6[1]-b*t
    p_5[2]=p6[2]-c*t
    list = [p_3,p_7,p_6,p_2,p_4,p_8,p_5,p_1]
    new = []
    for i in list:
        a = np.ndarray.tolist(i)
        new.append(a)
    # print(new)
    return new

#输入3个点，输出8个点
def point_3_to_8(args):                #坐标3-8
    min_x = args[0][0]
    max_x = args[2][0]
    min_y = args[0][1]
    max_y = args[2][1]
    min_z = args[0][2]
    max_z = args[2][2]

    a1 = [min_x,max_y,max_z]
    a2 = [max_x,max_y,max_z]                   #a前点,b后点,得到的数据类型是list
    a3 = [max_x,max_y,min_z]
    a4 = [min_x,max_y,min_z]
    b1 = [min_x,min_y,max_z]
    b2 = [max_x,min_y,max_z]
    b3 = [max_x,min_y,min_z]
    b4 = [min_x,min_y,min_z]

    args = [a1, a2, a3, a4, b1, b2, b3, b4]
    # print(args)
    return args

#输入8个点，输出3个点
def point_8_to_3(args):
    a = args[7]
    b = args[1]
    c = [(a[0]+b[0])/2,(a[1]+b[1])/2,(a[2]+b[2])/2]
    args = [a,c,b]

    return args


#输入管道信息，输出四条与流向平行的棱
def pipe_to_pipes(pipe):
    import numpy as np
    a = dian2(pipe[0],pipe[1],pipe[2],pipe[3])
    a1 = [a[0],a[1]]
    a2 = [a[3],a[2]]
    a3 = [a[4], a[5]]
    a4 = [a[7], a[6]]
    a5 = [pipe[0],pipe[1]]
    a6 = [a1,a2,a3,a4,a5]
    # print(a6)
    return a6

#输入起点终点宽高，输出8个点
def dian2(p1,p2,width,height):#第二种即两个中心点z不一样
    u=0
    p3=np.zeros(3)
    p4=np.zeros(3)
    p5=np.zeros(3)
    p6=np.zeros(3)
    p_1=np.zeros(3)
    p_2=np.zeros(3)
    p_3=np.zeros(3)
    p_4=np.zeros(3)
    p_5=np.zeros(3)
    p_6=np.zeros(3)
    p_7=np.zeros(3)
    p_8=np.zeros(3)
    if p1[1]-p2[1]==0:
        u=1
    if u==0:
        k=-(p1[0]-p2[0])/(p1[1]-p2[1])
        x=width/(2*(1+k**2)**0.5)
        p3[0]=p1[0]+x
        p3[1]=p1[1]+k*x
        p3[2]=p1[2]
        p4[0]=p1[0]-x
        p4[1]=p1[1]-k*x
        p4[2]=p1[2]
        p5[0]=p2[0]+x
        p5[1]=p2[1]+k*x
        p5[2]=p2[2]
        p6[0]=p2[0]-x
        p6[1]=p2[1]-k*x
        p6[2]=p2[2]
    else :
        p3[0]=p1[0]
        p3[1]=p1[1]+width/2
        p3[2]=p1[2]
        p4[0]=p1[0]
        p4[1]=p1[1]-width/2
        p4[2]=p1[2]
        p5[0]=p2[0]
        p5[1]=p2[1]+width/2
        p5[2]=p2[2]
        p6[0]=p2[0]
        p6[1]=p2[1]-width/2
        p6[2]=p2[2]
    a=(p2[1]-p1[1])*(p3[2]-p1[2])-(p3[1]-p1[1])*(p2[2]-p1[2])
    b=(p2[2]-p1[2])*(p3[0]-p1[0])-(p3[2]-p1[2])*(p2[0]-p1[0])
    c=(p2[0]-p1[0])*(p3[1]-p1[1])-(p3[0]-p1[0])*(p2[1]-p1[1])
    t=height/(2*(a**2+b**2+c**2)**0.5)
    p_3[0]=p3[0]+a*t
    p_3[1]=p3[1]+b*t
    p_3[2]=p3[2]+c*t
    p_2[0]=p3[0]-a*t
    p_2[1]=p3[1]-b*t
    p_2[2]=p3[2]-c*t
    p_4[0]=p4[0]+a*t
    p_4[1]=p4[1]+b*t
    p_4[2]=p4[2]+c*t
    p_1[0]=p4[0]-a*t
    p_1[1]=p4[1]-b*t
    p_1[2]=p4[2]-c*t
    p_7[0]=p5[0]+a*t
    p_7[1]=p5[1]+b*t
    p_7[2]=p5[2]+c*t
    p_6[0]=p5[0]-a*t
    p_6[1]=p5[1]-b*t
    p_6[2]=p5[2]-c*t
    p_8[0]=p6[0]+a*t
    p_8[1]=p6[1]+b*t
    p_8[2]=p6[2]+c*t
    p_5[0]=p6[0]-a*t
    p_5[1]=p6[1]-b*t
    p_5[2]=p6[2]-c*t
    list = [p_3,p_7,p_6,p_2,p_4,p_8,p_5,p_1]
    new = []
    for i in list:
        a = np.ndarray.tolist(i)
        new.append(a)
    # print(new)
    return new

#碰撞函数
def pengzhuang(mian,xian):
    n=mian[0]
    m=mian[1]
    o=mian[2]
    q1=xian[0]
    q2=xian[1]
    p=((n[1]-m[1])*(o[2]-m[2])-(o[1]-m[1])*(n[2]-m[2]),(n[2]-m[2])*(o[0]-m[0])-(o[2]-m[2])*(n[0]-m[0]),(n[0]-m[0])*(o[1]-m[1])-(o[0]-m[0])*(n[1]-m[1]))
    P=(p[0]**2+p[1]**2+p[2]**2)**0.5
    d1=(p[0]*(q1[0]-m[0])+p[1]*(q1[1]-m[1])+p[2]*(q1[2]-m[2]))
    d2=(p[0]*(q2[0]-m[0])+p[1]*(q2[1]-m[1])+p[2]*(q2[2]-m[2]))
    if d1==d2:
        return None
    else:
        u=d1/(d1-d2)
        t=[q1[0]-u*(q1[0]-q2[0]),q1[1]-u*(q1[1]-q2[1]),q1[2]-u*(q1[2]-q2[2])]
        j=-1
        h=0
        while j<2:
            j=j+1
            if t[j]>max(m[j],n[j],o[j])or t[j]<min(m[j],n[j],o[j]):
                h=1
            elif t[j]>max(q1[j],q2[j])or t[j]<min(q1[j],q2[j]):
                h=1
        if h==0:
            return t
        else:
            return None

#输入管道和障碍物，输出每段管道分别碰撞的障碍物
def pzjc(pipes,blocks):
    b = []
    b1 = []
    f = []
    g = []
    h = []
    k = []
    j = []
    o =[]
    block1 = []
    block2 = []
    block3 = []
    for w in range(len(pipes)):
        a = pipes[w]
        a1 = pipe_to_pipes(a)
        for v in range(len(blocks)):
            d1 = blocks[v]
            d2 = dian2(d1[0],d1[1],d1[2],d1[3])
            d = point_8_to_3(d2)
            e = point_3_to_surface_6(d)
            for p in range(len(a1)):
                i = 0
                while i < 6:
                    f = pengzhuang(e[i],a1[p])
                    # print(f)
                    if f != None:
                        g.append(blocks[v])
                        i += 1
                    else:
                        i+=1
            if len(g) != 0:
                k.append(g[0])
            else:
                k.append(o)

            g.clear()
            block1 = [k[i:i+len(blocks)] for i in range(0,len(k),len(blocks))]
    # print(block1)
    for sam in block1:
        if sam:
            block2 = []
            for x in sam:
                if x:
                    block2.append(x)
            block3.append(block2)


    # print("每个管道分别碰撞的障碍物为",block3)
    return block3

#输入3个点，输出起点、终点、宽高
def point_8_to_dian2(a):
    b = point_3_to_8(a)
    b1 = [b[0][0],(b[3][1]-b[7][1])/2,(b[0][2]+b[3][2])/2]
    b2 = [b1[0]+(b[2][0]-b[3][0]),b1[1],b1[2]]
    b3 = b[3][1]-b[7][1]
    b4 = b[0][2]-b[3][2]
    c = [b1,b2,b3,b4]
    # print(c)
    return c

p = [[[73635.986747802, 118955.886797115, 52900.0], [73635.986747802, 118955.886797115, 57400.0], 300.0, 300.0], [[84045.923132226, 118955.886797115, 52900.0], [84045.923132226, 118955.886797115, 57400.0], 300.0, 300.0], [[94427.253528686, 118955.886797115, 52900.0], [94427.253528686, 118955.886797115, 57400.0], 300.0, 300.0], [[105185.099027498, 118955.886797114, 52900.0], [105185.099027498, 118955.886797114, 57400.0], 300.0, 300.0], [[75754.656351344, 129155.886797115, 54911.932083198], [71654.656351344, 129155.886797115, 54911.932083198], 300.0, 700.0], [[86154.656351344, 129156.924490401, 54911.9], [82054.656351344, 129156.924490401, 54911.9], 300.0, 700.0], [[96593.832246613, 129156.924490401, 54911.9], [92493.832246613, 129156.924490401, 54911.9], 300.0, 700.0], [[107393.832246617, 129156.924490401, 54911.9], [103293.832246617, 129156.924490401, 54911.9], 300.0, 700.0], [[75009.824255584, 139908.640673971, 55150.0], [72489.824255584, 139908.640673972, 55150.0], 100.0, 4500.0], [[85509.824255584, 139908.640673971, 55150.0], [82989.824255584, 139908.640673972, 55150.0], 100.0, 4500.0], [[96009.824255584, 139908.640673971, 55150.0], [93489.824255584, 139908.640673972, 55150.0], 100.0, 4500.0], [[106509.824255584, 139908.640673971, 55150.0], [103989.824255584, 139908.640673972, 55150.0], 100.0, 4500.0]]
bs = [[[73635.986747802, 118955.886797115, 52900.0], [73635.986747802, 118955.886797115, 57400.0], 300.0, 300.0], [[84045.923132226, 118955.886797115, 52900.0], [84045.923132226, 118955.886797115, 57400.0], 300.0, 300.0], [[94427.253528686, 118955.886797115, 52900.0], [94427.253528686, 118955.886797115, 57400.0], 300.0, 300.0], [[105185.099027498, 118955.886797114, 52900.0], [105185.099027498, 118955.886797114, 57400.0], 300.0, 300.0], [[56357.415154532, 45996.530662299, 57050.0], [56357.415154532, 45996.530662299, 57050.0], 1000.0, 700.0], [[75754.656351344, 129155.886797115, 54911.932083198], [71654.656351344, 129155.886797115, 54911.932083198], 300.0, 700.0], [[86154.656351344, 129156.924490401, 54911.9], [82054.656351344, 129156.924490401, 54911.9], 300.0, 700.0], [[96593.832246613, 129156.924490401, 54911.9], [92493.832246613, 129156.924490401, 54911.9], 300.0, 700.0], [[107393.832246617, 129156.924490401, 54911.9], [103293.832246617, 129156.924490401, 54911.9], 300.0, 700.0], [[75009.824255584, 139908.640673971, 55150.0], [72489.824255584, 139908.640673972, 55150.0], 100.0, 4500.0], [[85509.824255584, 139908.640673971, 55150.0], [82989.824255584, 139908.640673972, 55150.0], 100.0, 4500.0], [[96009.824255584, 139908.640673971, 55150.0], [93489.824255584, 139908.640673972, 55150.0], 100.0, 4500.0], [[106509.824255584, 139908.640673971, 55150.0], [103989.824255584, 139908.640673972, 55150.0], 100.0, 4500.0]]


# 输入一列管道和障碍物，输出每段管道绑定的障碍物
def pipes_to_area(p, bs):
    import numpy as np
    from numpy.core.fromnumeric import shape
    import copy
    a = []
    b = []
    c = []
    c1 = []
    c2 = []
    c3 = []
    c4 = []
    d1 = []
    f1 = []
    f2 = []
    pipes = []
    blocks = []
    e1 = []
    e2 = []
    e3 = []
    h1 = []
    h2 = []
    h3 = []
    # h4 = []
    # h5 = []
    # h6 = []
    area = []
    block = []
    block1 = []
    block2 = []
    block3 = []
    block4 = []
    block5 = []
    temp = 1000
    for i in range(len(p)):
        p1 = p[i]
        p3 = dian2(p1[0], p1[1], p1[2], p1[3])
        pipes.append(p3)
    for i in range(len(bs)):
        b1 = bs[i]
        b2 = dian2(b1[0], b1[1], b1[2], b1[3])
        blocks.append(b2)
    for w in range(len(pipes)):
        a = pipes[w]
        b = np.array(a)
        d = np.array(b.reshape(int(b.size / 3), 3))
        e = d.tolist()
        for i in range(len(e)):
            e1.append(e[i][0])
            e2.append(e[i][1])
            e3.append(e[i][2])
        g1 = [min(e1), min(e2), min(e3)]
        f1.append(g1)
        g2 = [max(e1), max(e2), max(e3)]
        f2.append(g2)
        d1.append([g1, g2])
        d2 = copy.deepcopy(d1)
        c1.extend(d2)
        if c1[0][0][0] > temp and c1[0][0][1] > temp and c1[0][0][2] > temp:
            c2.append([[c1[0][0][0] - temp, c1[0][0][1] - temp, c1[0][0][2] - temp],
                       [c1[0][1][0] + temp, c1[0][1][1] + temp, c1[0][1][2] + temp]])
        else:
            c2.append([c1[0][0], [c[0][1][0] + temp, c1[0][1][1] + temp, c1[0][1][2] + temp]])
        for v in range(len(blocks)):
            f = copy.deepcopy(blocks[v])
            f.append([(f[0][0] + f[1][0]) / 2, (f[0][1] + f[1][1]) / 2, (f[0][2] + f[2][2]) / 2])
            i = 0
            while i <= 8:
                if c2[0][0][0] <= f[i][0] <= c2[0][1][0] and c2[0][0][1] <= f[i][1] <= c2[0][1][1] and c2[0][0][2] <= \
                        f[i][2] <= c2[0][1][2]:
                    if len(area) == 0:
                        area.append(bs[v])
                        # print(area1)
                i += 1
            block.extend(area)
            f.clear()
            block1 = copy.deepcopy(block)
            area.clear()
        block.clear()
        block2.extend(block1)
        c3 = [(c2[0][1][0] + c2[0][0][0]) / 2, (c2[0][1][1] + c2[0][0][1]) / 2, (c2[0][1][2] + c2[0][0][2]) / 2]
        c4 = [c2[0][0], c3, c2[0][1]]
        h1.extend(c4)
        h2 = point_8_to_dian2(h1)
        # print(h2)
        block3 = pzjc([h2],bs)
        for j in block3:
            if j not in block2:
                block2.extend(j)
        block4 = copy.deepcopy(block2)
        # print(block4)
        h3.append(h2)
        c1.clear()
        c2.clear()
        c3.clear()
        d1.clear()
        e1.clear()
        e2.clear()
        e3.clear()
        h1.clear()
        block2.clear()
        block5.append(block4)
    print(block5)
    return block3

# 输入一列管道和障碍物，输出一列管道绑定的障碍物
def pipes_to_region(p,bs):
    import numpy as np
    from numpy.core.fromnumeric import shape
    import copy
    a = []
    b = []
    c = []
    c3 = []
    c4 = []
    h1 = []
    h2 = []
    pipes = []
    blocks = []
    e1 = []
    e2 = []
    e3 = []
    area = []
    area1 = []
    area2 = []
    area3 = []
    block = []
    block1 = []
    block2 = []
    block3 = []
    temp = 1000
    for j in range(len(p)):
        p2 = p[j]
        p3 = dian2(p2[0],p2[1],p2[2],p2[3])
        pipes.append(p3)
    for i in range(len(bs)):
        b1 = bs[i]
        b2 = dian2(b1[0],b1[1],b1[2],b1[3])
        blocks.append(b2)
    a = pipes
    b=np.array(a)
    d=np.array(b.reshape(int(b.size/3),3))
    e = d.tolist()
    # print(e)
    for i in range(len(e)):
        e1.append(e[i][0])
        e2.append(e[i][1])
        e3.append(e[i][2])
    g1 = [min(e1),min(e2),min(e3)]
    g2 = [max(e1),max(e2),max(e3)]
    c.append([g1,g2])
    if c[0][0][0] > temp and c[0][0][1] > temp and c[0][0][2 ]> temp:
        area.append([[c[0][0][0]-temp,c[0][0][1]-temp,c[0][0][2]-temp],[c[0][1][0]+temp,c[0][1][1]+temp,c[0][1][2]+temp]])
    else:
        area.append([c[0][0],[c[0][1][0] + temp, c[0][1][1] + temp, c[0][1][2] + temp]])
        c.clear()
        e1.clear()
        e2.clear()
        e3.clear()
    c3 = [(area[0][1][0] + area[0][0][0]) / 2, (area[0][1][1] + area[0][0][1]) / 2, (area[0][1][2] + area[0][0][2]) / 2]
    c4 = [area[0][0], c3, area[0][1]]
    h1.extend(c4)
    h2 = point_8_to_dian2(h1)
    block3 = pzjc([h2], bs)
    for v in range(len(blocks)):
        f = copy.deepcopy(blocks[v])
        f.append([(f[0][0]+f[1][0])/2,(f[0][1]+f[1][1])/2,(f[0][2]+f[2][2])/2])
        i = 0
        while i<=8:
            if area[0][0][0] <= f[i][0] <= area[0][1][0] and area[0][0][1] <= f[i][1] <= area[0][1][1] and area[0][0][2] <= f[i][2] <= area[0][1][2]:
                if len(area1) == 0:
                    area1.append(bs[v])
                    # print(area1)

            i+=1
        block.extend(area1)
        f.clear()
        block1 = copy.deepcopy(block)
        area1.clear()
    for j in block1:
        if j not in block1:
            block1.extend(j)
    block.clear()
    block2.extend(block1)
    area.clear()
    print("所有管道区域绑定的障碍物为",block2)
    return block2

# pipes_to_area(p,bs)
# pipes_to_region(p,bs)